#include "stdafx.h"
#include "ODESolver.h"
#include "ForwardDynamics.h"
#ifdef _MKL_IMPLEMENT
#include <mkl_cblas.h>
#include <mkl_spblas.h>
#include <mkl_rci.h>
#include <mkl_blas.h>
#include <mkl_service.h>
#else 
#include "JieBlas.h"
#endif
#include "DecoLogger.h"

void ODESolver::Solve(double timeStep)
{
	mkl_domain_set_num_threads( 1, MKL_ALL );

	switch(mType)
	{
	case ST_ExplicitEuler:
		ExplicitEulerStep(timeStep);
		break;
	case ST_RK4:
		RK4Step(timeStep);
		break;

	default:
		assert(0);
	}
}
void ODESolver::ExplicitEulerStep(double timeStep)
{
	size_t dim = mDynamicSys->GetDim();
	double* deriv = new double[dim];
	mDynamicSys->CalculateDeriv(deriv);
	double* currentState = new double[dim];
	mDynamicSys->GetState(currentState);

#ifdef _MKL_IMPLEMENT
	cblas_daxpy(static_cast<int>(dim), timeStep, deriv, 1, currentState, 1);
#else
	myblas_daxpy(static_cast<int>(dim), timeStep, deriv, currentState);
#endif
	mDynamicSys->SetState(currentState);
	delete[] deriv;
	delete[] currentState;
}


void ODESolver::RK4Step(double timeStep)
{
	int dim = static_cast<int>(mDynamicSys->GetDim());
	double* currentState = new double[dim];
	double* tmpState = new double[dim];
	double* k1 = new double[dim];
	double* k2 = new double[dim];
	double* k3 = new double[dim];
	double* k4 = new double[dim];


	mDynamicSys->GetState(currentState);
	mDynamicSys->CalculateDeriv(k1);
#ifdef _MKL_IMPLEMENT
	cblas_dcopy(dim, currentState, 1, tmpState, 1);
	cblas_daxpy(dim, timeStep / 2, k1, 1, tmpState, 1);
#else
	myblas_dcopy(dim, currentState, tmpState);
	myblas_daxpy(dim, timeStep / 2, k1, tmpState);

#endif
	mDynamicSys->SetState(tmpState);

	mDynamicSys->CalculateDeriv(k2);
#ifdef _MKL_IMPLEMENT
	cblas_dcopy(dim, currentState, 1, tmpState, 1);
	cblas_daxpy(dim, timeStep / 2, k2, 1, tmpState, 1);
#else
	myblas_dcopy(dim, currentState, tmpState);
	myblas_daxpy(dim, timeStep / 2, k2, tmpState);
#endif
	mDynamicSys->SetState(tmpState);

	mDynamicSys->CalculateDeriv(k3);
#ifdef _MKL_IMPLEMENT
	cblas_dcopy(dim, currentState, 1, tmpState, 1);
	cblas_daxpy(dim, timeStep, k3, 1, tmpState, 1);
#else
	myblas_dcopy(dim, currentState, tmpState);
	myblas_daxpy(dim, timeStep, k3, tmpState);
#endif
	mDynamicSys->SetState(tmpState);
 
	mDynamicSys->CalculateDeriv(k4);

#ifdef _MKL_IMPLEMENT
	cblas_daxpy(dim, 1.f / 6.f * timeStep, k1, 1, currentState, 1);
	cblas_daxpy(dim, 1.f / 3.f * timeStep, k2, 1, currentState, 1);
	cblas_daxpy(dim, 1.f / 3.f * timeStep, k3, 1, currentState, 1);
	cblas_daxpy(dim, 1.f / 6.f * timeStep, k4, 1, currentState, 1);
#else
	myblas_daxpy(dim, 1.f / 6.f * timeStep, k1, currentState);
	myblas_daxpy(dim, 1.f / 3.f * timeStep, k2, currentState);
	myblas_daxpy(dim, 1.f / 3.f * timeStep, k3, currentState);
	myblas_daxpy(dim, 1.f / 6.f * timeStep, k4, currentState);
#endif
	mDynamicSys->SetState(currentState);


	delete[] k1;
	delete[] k2;
	delete[] k3;
	delete[] k4;
	delete[] currentState;
	delete[] tmpState;
}
